#include "LQ_demo.hpp"

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
 * @函数名称：void EncoderDemo()
 * @功能说明：编码器测试(寄存器，PWM 控制器实现)
 * @参数说明：无
 * @函数返回：无
 * @调用方法：EncoderDemo();
 * @备注说明：可以搭配电机，也可手转查看编码器返回值
 QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
void EncoderDemo()
{
    // 电机PWM
    SetPWM LMotorPWM(PWM2, 100000, 35000, "inversed");   //66
    SetPWM RMotorPWM(PWM1, 100000, 35000, "inversed");   //65
    // 电机方向GPIO
    HWGpio LMotoDir(74, 1);
    HWGpio RMotoDir(75, 1);
    // 初始化编码器计数通道
    LS_PwmEncoder EncodersL(0, 73);
    LS_PwmEncoder EncodersR(3, 72);
    // 初始化电机
    LMotorPWM.Enable();
    RMotorPWM.Enable();
    while(1)
    {
        cout << "left = " << EncodersL.Update() << ", right = " << EncodersR.Update() << endl;
        usleep(1000);
    }
}


// #include <main.hpp>

// int main(){
//     /* 舵机PWM */
//     GtimPwm ServoPWM1(88, 2, LS_GTIM_INVERSED, 2000000 - 1, 155000); // TIM2_CH2 GPIO88   舵机   
//     ServoPWM1.Enable();
//     while (1)
//     {
//         ServoPWM1.SetDutyCycle(140000);
//         printf("PWM Set %d\n", 130000);
//         sleep(1);
//         ServoPWM1.SetDutyCycle(157500);
//         printf("PWM Set %d\n", 150000);
//         sleep(1);
//         ServoPWM1.SetDutyCycle(174500);
//         printf("PWM Set %d\n", 170000);
//         sleep(1);

//     }
// }
